// 动态广播
#include "geometry_msgs/msg/transform_stamped.hpp"
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_broadcaster.hpp>
#include <turtlesim_msgs/msg/pose.hpp>

using turtlesim_msgs::msg::Pose;

// DynamicBroadcaster 动态广播器
class DynamicBroadcaster : public rclcpp::Node {
public:
  DynamicBroadcaster() : Node("dynamic_broadcaster") {
    // 创建动态广播器对象
    dynamic_broadcaster_ =
        std::make_shared<tf2_ros::TransformBroadcaster>(*this);
    // 创建乌龟位置姿态的订阅者
    subscription_ = this->create_subscription<Pose>(
        "/turtle1/pose", 10,
        std::bind(&DynamicBroadcaster::topic_callback, this,
                  std::placeholders::_1));
  }

  // 发送动态变换 把乌龟的位置姿态发布出去 给 tf2
  void sendTransform(const geometry_msgs::msg::TransformStamped &t) {

    // 发送动态变换
    dynamic_broadcaster_->sendTransform(t);
    RCLCPP_INFO(this->get_logger(), "Sending dynamic transform");
  }

private:
  // 创建动态广播器对象的智能指针
  std::shared_ptr<tf2_ros::TransformBroadcaster> dynamic_broadcaster_;

  // 创建乌龟位置姿态的订阅者
  rclcpp::Subscription<Pose>::SharedPtr subscription_;

  // 订阅回调函数
  void topic_callback(const Pose &msg) {

    RCLCPP_INFO(this->get_logger(), "I heard: x: %.2f, y: %.2f, theta: %.2f",
                msg.x, msg.y, msg.theta);

    // 创建并发布动态变换
    geometry_msgs::msg::TransformStamped t;
    // 设置时间戳、父子坐标系、平移和旋转
    t.header.stamp = this->get_clock()->now();
    // 父坐标系
    t.header.frame_id = "base_link";
    t.child_frame_id = "talk";
    // 平移和旋转
    t.transform.translation.x = msg.x;
    t.transform.translation.y = msg.y;
    t.transform.translation.z = 0.0;

    // 角速度换算四元数
    double theta = msg.theta; // 角度值

    // 计算四元数的值
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = sin(theta / 2.0);
    t.transform.rotation.w = cos(theta / 2.0);

    // 发送动态变换
    sendTransform(t);
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DynamicBroadcaster>());
  rclcpp::shutdown();
  return 0;
}
